//
// Created by bourne on 2024/4/15.
//

#include "debug_utils/joy.h"

DroneJoyStick::DroneJoyStick() {}

DroneJoyStick::~DroneJoyStick() {}

void DroneJoyStick::init(ros::NodeHandle& nh) {
  nh_ = nh;

  nh_.param("drone_id", drone_id_, -1);
  nh.param("active_button", activate_button_, -1);



  activate_buttons_.clear();
  activate_buttons_ = {0, 1, 2, 3};
  button_names_.clear();
  button_names_ = {"A", "B", "X", "Y"};

  nh_.param("init_x", init_x_, -1.0);
  nh_.param("init_y", init_y_, -1.0);
  nh_.param("init_z", init_z_, -1.0);


  activated_ = false;

  // Control parameter
  cmd_.kx = { 5.7, 5.7, 6.2 };
  cmd_.kv = { 3.4, 3.4, 4.0 };

  cmd_.header.stamp = ros::Time::now();
  cmd_.header.frame_id = "world";
  cmd_.trajectory_flag = quadrotor_msgs::PositionCommand::TRAJECTORY_STATUS_READY;
  cmd_.trajectory_id = 0;
  cmd_.position.x = init_x_;
  cmd_.position.y = init_y_;
  cmd_.position.z = init_z_;
  cmd_.velocity.x = 0.0;
  cmd_.velocity.y = 0.0;
  cmd_.velocity.z = 0.0;
  cmd_.acceleration.x = 0.0;
  cmd_.acceleration.y = 0.0;
  cmd_.acceleration.z = 0.0;
  cmd_.yaw = 0.0;
  cmd_.yaw_dot = 0.0;

  odom_sub_ = nh_.subscribe("/odom", 1, &DroneJoyStick::odomCallback, this);
  joy_sub_ = nh_.subscribe("/joy", 1, &DroneJoyStick::joyCallback, this);

  cmd_pub_ = nh_.advertise<quadrotor_msgs::PositionCommand>("/position_cmd", 50);
  state_pub_ = nh_.advertise<visualization_msgs::Marker>("/state_vis", 50);

  cmd_timer_ = nh_.createTimer(ros::Duration(0.01), &DroneJoyStick::cmdTimerCallback, this);
  state_timer_ = nh_.createTimer(ros::Duration(0.01), &DroneJoyStick::stateTimerCallback, this);
}

void DroneJoyStick::odomCallback(const nav_msgs::OdometryConstPtr &msg) {
  pos_[0] = msg->pose.pose.position.x;
  pos_[1] = msg->pose.pose.position.y;
  pos_[2] = msg->pose.pose.position.z;

  //↓将旋转矩阵转换为欧拉角
  double roll,pitch,yaw;
  tf::Quaternion q;
  tf::quaternionMsgToTF(msg->pose.pose.orientation,q);
  tf::Matrix3x3(q).getRPY(roll,pitch,yaw);
  yaw_ = yaw;
}

void DroneJoyStick::joyCallback(const sensor_msgs::JoyConstPtr &msg) {
  // back and forth
  if (fabs(msg->axes[1]) > 0.2) {
    joy_cmd_[0] = msg->axes[1];
  } else {
    joy_cmd_[0] = 0.0;
  }

  // left and right
  if (fabs(msg->axes[0]) > 0.2) {
    joy_cmd_[1] = msg->axes[0];
  } else {
    joy_cmd_[1] = 0.0;
  }

  // up and down
  if (fabs(msg->axes[4]) > 0.7) {
    joy_cmd_[2] = msg->axes[4];
  } else {
    joy_cmd_[2] = 0.0;
  }

  // yaw
  if (fabs(msg->axes[3]) > 0.2) {
    joy_cmd_[3] = msg->axes[3];
  } else {
    joy_cmd_[3] = 0.0;
  }

  for(int i = 0; i <activate_buttons_.size(); i++) {
    if (msg->buttons[activate_buttons_[i]]==1) {
      if (activate_buttons_[i] == activate_button_) {
        activated_ = true;
      } else {
        activated_ = false;
      }
    }
  }
}

void DroneJoyStick::cmdTimerCallback(const ros::TimerEvent &e) {
  if (activated_) {
    cmd_.header.stamp = ros::Time::now();
    cmd_.trajectory_id++;
    cmd_.position.x += joy_cmd_[0] * 0.03;
    cmd_.position.y += joy_cmd_[1] * 0.03;
    cmd_.position.z += joy_cmd_[2] * 0.03;
//  cmd_.velocity.x = 0.0;
//  cmd_.velocity.y = 0.0;
//  cmd_.velocity.z = 0.0;
//  cmd_.acceleration.x = 0.0;
//  cmd_.acceleration.y = 0.0;
//  cmd_.acceleration.z = 0.0;
    // todo wrap yaw
    cmd_.yaw += joy_cmd_[3] * 0.03;
//  cmd_.yaw_dot = 0.0;
    cmd_pub_.publish(cmd_);
  }
}

void DroneJoyStick::stateTimerCallback(const ros::TimerEvent &e) {
  // 设置一个Marker消息
  visualization_msgs::Marker text_marker;
  text_marker.header.frame_id = "world"; // 假设我们使用地图坐标系
  text_marker.header.stamp = ros::Time::now();
  text_marker.ns = "text"; // 命名空间
  text_marker.id = 0; // 唯一标识符
  text_marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING; // 文本类型
  text_marker.action = visualization_msgs::Marker::ADD; // 添加标记

  // 设置文本内容
  std::string text = std::to_string(drone_id_) + "_" + button_names_[activate_button_];
  text_marker.text = text;

  // 设置位置
  text_marker.pose.position.x = pos_[0];
  text_marker.pose.position.y = pos_[1];
  text_marker.pose.position.z = pos_[2] + 0.3;

  // 设置方向（可选，因为TEXT_VIEW_FACING会自动面向观察者）
  text_marker.pose.orientation.x = 0.0;
  text_marker.pose.orientation.y = 0.0;
  text_marker.pose.orientation.z = 0.0;
  text_marker.pose.orientation.w = 1.0;

  // 设置颜色和缩放
  if (activated_) {
    text_marker.color.r = 1.0;
    text_marker.color.g = 0.0;
    text_marker.color.b = 0.0;
    text_marker.color.a = 1.0; // 不透明度
    text_marker.scale.x = 0.6;
    text_marker.scale.y = 0.6;
    text_marker.scale.z = 0.6; // 文本深度
  } else {
    text_marker.color.r = 0.0;
    text_marker.color.g = 0.0;
    text_marker.color.b = 0.0;
    text_marker.color.a = 1.0; // 不透明度
    text_marker.scale.x = 0.4;
    text_marker.scale.y = 0.4;
    text_marker.scale.z = 0.4; // 文本深度
  }

  state_pub_.publish(text_marker);
}




